%% Rotate a triangle
% Rotate a triangle through 90*, on at a time
tic
clear;
close all;

%% Script Configuration

%gauge2 desired reading.
g2dr=-1; 

gauge.frict=.90; % Friction of the gauge
gauge.accel=.025; % Amount of push on the gauge

%% Import Image
fig=figure();
img_gauge=imread('gauge','jpg');


%% Create and scale the plot
h_img=image(img_gauge);
hold on;
h_par_axes=get(h_img,'Parent');
set(h_par_axes,'YDir','normal');

%% Figure config
set(fig,'Position',[200 200 640 480]);

%% Axis config
% Get the current axes
fig_axis=get(fig,'CurrentAxes');
% Set axes to use pixels
set(fig_axis,'Units','pixels');
set(fig_axis,'Position',[0 0 640 480+1]);


%% Define shapes
% Gauge One
tri = [85,396;
       90,312; 
       95,396];

%% Struct Template
% The struct should have the following data in it
%
%   shape: All the verticies of the shape
%   center: [x,y] of the center of the shape
%   heading: The current heading of the indicator, zeroed at creation
%   rotation: Current speed of rotation.
%   heading_max: Maximum position rotation
%   heading_min: Maximum negative rotation (-2 allows for 2 degrees
%       clockwise from zero.
%   weight: the resistance of the needle to new speeds.

tri_str = struct(   'shape',[ 89,134 ; 84,227 ; 94,227],...
                    'center',[89,210],...
                    'heading',0,...
                    'rotation',0,...
                    'heading_max',270,...
                    'heading_min',0,...
                    'weight',7,...
                    'handler',0);

   
   
% Initialize the variables   
h_g_1=fill(tri(:,1),tri(:,2),'w');
tri_str.handler=fill(tri_str.shape(:,1),tri_str.shape(:,2),'w');
txt_rot=text(212,230,sprintf('Rotation: %d',tri_str.rotation));
txt_head=text(212,215,sprintf('Heading: %d',tri_str.rotation));

%% Labels
text(200,456,'Old Method');
text(200,288,'New Method');
txt_step=text(11,25,'Index: ');

%% Loop
for i=1:(1000)
    %% Make up some intresting stuff for the needle to do
    
    switch i
        case 100
            g2dr=1;
        case 200
            g2dr=2;
        case 300
            g2dr=3;
        case 400
            g2dr=4;
        case 500
            g2dr=5;
        case 600
            g2dr=10;
        otherwise
    end
    
    %% Calculation new rotation
    %              \/ - Friction?
    % new = old * friction loss + acceleration * ( desired - heading) / weight
    tri_str.rotation = tri_str.rotation * gauge.frict + gauge.accel * (g2dr*25.4 - tri_str.heading)/tri_str.weight;
    
    %% Rotate shapes
    tri_str.shape = poly_rotate2(tri_str.shape,tri_str.center(1),tri_str.center(2),tri_str.rotation);
    
    %% Update Heading
    tri_str.heading = tri_str.heading + tri_str.rotation;
    
    
    %% Draw results
    set(tri_str.handler,'Vertices',tri_str.shape);
    
    
    %% Debuging Text
    set(txt_step,'string',sprintf('Index: %d',i));
    set(txt_rot,'string',sprintf('Rotation: %0.2f',tri_str.rotation));
    set(txt_head,'string',sprintf('Heading: %0.2f',tri_str.heading));
    
    %% Finish old method
    tri=poly_rotate2(tri,90,390,1);
    delete(h_g_1);
    h_g_1=fill(tri(:,1),tri(:,2),'w');
    drawnow;
    M(i)=getframe;
end

% movie2avi(M,'guage_testv3','FPS',30,'compression','none');
toc

